Rapidly-exploring Random Belief Trees for Motion Planning Under Uncertainty

In this paper we address the problem of motion planning in the presence of state uncertainty, also known as planning in belief space. The work is motivated by planning domains involving nontrivial dynamics, spatially varying measurement properties, and obstacle constraints. To make the problem tractable, we restrict the motion plan to a nominal trajectory stabilized with a linear estimator and controller. This allows us to predict distributions over future states given a candidate nominal trajectory. Using these distributions to ensure a bounded probability of collision, the algorithm incrementally constructs a graph of trajectories through state space, while efficiently searching over candidate paths through the graph at each iteration. This process results in a search tree in belief space that provably converges to the optimal path. We analyze the algorithm theoretically and also provide simulation results demonstrating its utility for balancing information gathering to reduce uncertainty and finding low cost paths.United States. Office of Naval Research (MURI N00014-09-1-1052

State estimation for aggressive flight in GPS-denied environments using onboard sensing

In this paper we present a state estimation method based on an inertial measurement unit (IMU) and a planar laser range finder suitable for use in real-time on a fixed-wing micro air vehicle (MAV). The algorithm is capable of maintaing accurate state estimates during aggressive flight in unstructured 3D environments without the use of an external positioning system. Our localization algorithm is based on an extension of the Gaussian Particle Filter. We partition the state according to measurement independence relationships and then calculate a pseudo-linear update which allows us to use 20x fewer particles than a naive implementation to achieve similar accuracy in the state estimate. We also propose a multi-step forward fitting method to identify the noise parameters of the IMU and compare results with and without accurate position measurements. Our process and measurement models integrate naturally with an exponential coordinates representation of the attitude uncertainty. We demonstrate our algorithms experimentally on a fixed-wing vehicle flying in a challenging indoor environment

CELLO: A fast algorithm for Covariance Estimation

We present CELLO (Covariance Estimation and Learning through Likelihood Optimization), an algorithm for predicting the covariances of measurements based on any available informative features. This algorithm is intended to improve the accuracy and reliability of on-line state estimation by providing a principled way to extend the conventional fixed-covariance Gaussian measurement model. We show that in experiments, CELLO learns to predict measurement covariances that agree with empirical covariances obtained by manually annotating sensor regimes. We also show that using the learned covariances during filtering provides substantial quantitative improvement to the overall state estimate. © 2013 IEEE.United States. National Aeronautics and Space AdministrationSiemens Corporate ResearchUnited States. Office of Naval Research. Multidisciplinary University Research InitiativeMicro Autonomous Consortium Systems and Technolog

Aggressive flight of fixed-wing and quadrotor aircraft in dense indoor environments

In this paper, we describe trajectory planning and state estimation algorithms for aggressive flight of micro aerial vehicles in known, obstacle-dense environments. Finding aggressive but dynamically feasible and collision-free trajectories in cluttered environments requires trajectory optimization and state estimation in the full state space of the vehicle, which is usually computationally infeasible on realistic timescales for real vehicles and sensors. We first build on previous work of van Nieuwstadt and Murray and Mellinger and Kumar, to show how a search process can be coupled with optimization in the output space of a differentially flat vehicle model to find aggressive trajectories that utilize the full maneuvering capabilities of a quadrotor. We further extend this work to vehicles with complex, Dubins-type dynamics and present a novel trajectory representation called a “Dubins–Polynomial trajectory”, which allows us to optimize trajectories for fixed-wing vehicles. To provide accurate state estimation for aggressive flight, we show how the Gaussian particle filter can be extended to allow laser rangefinder localization to be combined with a Kalman filter. This formulation allows similar estimation accuracy to particle filtering in the full vehicle state but with an order of magnitude more efficiency. We conclude with experiments demonstrating the execution of quadrotor and fixed-wing trajectories in cluttered environments. We show results of aggressive flight at speeds of up to 8 m/s for the quadrotor and 11 m/s for the fixed-wing aircraft.Micro Autonomous Consortium Systems and TechnologyUnited States. Office of Naval Research. Multidisciplinary University Research Initiative (Grant N00014-09-1-1052)American Society for Engineering Education. National Defense Science and Engineering Graduate Fellowshi